#include "m14kc.h"

#define   	ICache					(0x00)
#define   	DCache					(0x01)
#define		IndexStoreTag			(0x2 << 2)
#define   	IndexStoreTag_ICache	(IndexStoreTag |ICache)
#define   	IndexStoreTag_DCache	(IndexStoreTag | DCache)

/************************************************************/
	.globl  	en_cache		# Linker global variable
	.ent    	en_cache		# Linker start of function
en_cache:
	.set    	noreorder
	li      	t0,  0x03
	mtc0    	t0,  CP0_CONFIG
	jr      	ra
	nop
	.set    	reorder
	.end    	en_cache
        
/*************************************************************/
	.globl  	cache_init      # Linker global variable
	.ent    	cache_init      # Linker start of function
cache_init:
	.set    	noreorder
	# Determine I-Cache size
	mfc0    	t2,  CP0_CONFIG1             # .word 0x400a8001

	# I-Cache line size
	sll     	t3,  t2, 10
	srl    	t3,  29

	# Skip ahead if No I-Cache exist.
	# CP0_CONFIG1 IL field is 0, there is no I-Cache present.
	beq     	t3,  zero, 10f
	nop

	li	        t6,  2
	sllv		t3,  t6, t3           # Now have t3 is the I-Cache line size. 

	sll     	t4,  t2, 7
	srl     	t4,  29
	li      	t6,  64
	sllv    	t4,  t6, t4            # t4: I-Cache Sets per way

	sll     	t5,  t2, 13
	srl     	t5,  29           		# I-Cache Associativity -1.
	add   		t5,  1
	mul     	t4,  t4, t5           	# t4: Total number of sets

	lui     	t6,  0x8000   		# Get a KSeg0 address for cacheops

	# Clear TagLo/TagHi registers
	mtc0    	zero,  CP0_TAGLO
	mtc0    	zero,  CP0_TAGHI

	move    	t7,  t4

	# Index Store Tag Cache Op
	# Will invalidate the tag entry, clear the lock bit, and clear the LRU bit
1:      
	cache   	IndexStoreTag_ICache,  0(t6)
	add     	t7,  -1                 # Decrement set counter

	bne     	t7,  zero, 1b
	add     	t6,  t3                # Get next line address (branch delay slot).
        
	# Now go through and invalidate the D-Cache.		
10:
	# Isolate D-Cache Line Size.
	sll     	t3,  t2, 19
	srl     	t3,  29

	# Skip ahead if No D-Cache.
	beq     	t3,  zero, 10f
	nop

	li      	t6,  2
	sllv    	t3,  t6, t3           # Now have t3 is the D-Cache line size. 

	sll     	t4,  t2, 16
	srl     	t4,  29
	li     	 	t6,  64
	sllv    	t4,  t6, t4           # t4: D-Cache Sets per way.

	sll     	t5,  t2, 22
	srl     	t5,  29                 # D-Cache Associativity -1
	add     	t5,  1

	mul     	t4,  t4, t5           # Now t4: total number of sets

	lui     	t6,  0x8000             # Get a KSeg0 address for cacheops

	# Clear TagLo/TagHi registers
	mtc0    	zero,  CP0_TAGLO
	mtc0    	zero,  CP0_TAGHI

	move    	t7,  t4

	# Index Store Tag Cache Op
	# Will invalidate the tag entry, clear the lock bit, and clear the LRU bit
1:      
	cache   	IndexStoreTag_DCache, 0(t6)
	add     	t7,  -1                 # Decrement set counter
	bne     	t7,  zero, 1b
	add     	t6,  t3                # Get next line address
10:
	# Set KSeg0 to cacheable Config.K0   config[2:0]=3
	mfc0    	t2,  CP0_CONFIG
	li      	t3,  0x7
	not     	t3
	and     	t2,  t3
	or      	t2,  0x3
	mtc0    	t2,  CP0_CONFIG
	#VSI cache init end
	jr      	ra
	nop
	.set    	reorder
	.end    	cache_init

/**************************************************************************************/
	.globl  	dis_cache       # Linker global variable
	.ent    	dis_cache       # Linker start of function
dis_cache:
	.set    	noreorder
	li      	t0,  0x02
	mtc0    	t0,  CP0_CONFIG

	jr      	ra
	nop
	.set    	reorder
	.end    	dis_cache
